#include <errno.h>
#include <stddef.h>
#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/sys/printk.h>
#include <zephyr/sys/slist.h>
#include <zephyr/types.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/sys/byteorder.h>

#include "ble_multi_central.h"

LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);

#define BT_UUID_NB_DATA_SERVICE                                                \
  BT_UUID_DECLARE_128(                                                         \
      BT_UUID_128_ENCODE(0x0000ff01, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212))

#define BT_UUID_NB_DATA_SERVICE_NOTIFY                                         \
  BT_UUID_DECLARE_128(                                                         \
      BT_UUID_128_ENCODE(0x0000ff03, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212))

#define BT_UUID_NB_DATA_SERVICE_WRITE                                          \
  BT_UUID_DECLARE_128(                                                         \
      BT_UUID_128_ENCODE(0x0000ff02, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212))

// 要查找的服务UUID列表
static const bt_uuid_t *target_services[] = {BT_UUID_NB_DATA_SERVICE};

// 设备地址结构体，用于保存设备信息
struct device_info {
  bt_addr_le_t addr;
  uint8_t dev_idx;
  bool connected;
  bool discovery_complete;
  const char *name;
  int retry_count;
};

// 定义两个测试设备
static struct device_info test_devices[] = {
    {
        .addr =
            {
                .type = BT_ADDR_LE_RANDOM,
                .a =
                    {
                        .val = {0x7F, 0x3D, 0x44, 0x3B, 0xC8,
                                0xCD}, // 第二个设备地址，需要替换为实际设备地址
                    },
            },
        .dev_idx = 0xFF, // 未分配
        .connected = false,
        .discovery_complete = false,
        .name = "V3100",
        .retry_count = 0,
    },
    // {
    //     .addr =
    //         {
    //             .type = BT_ADDR_LE_RANDOM,
    //             .a =
    //                 {
    //                     .val = {0x80, 0xF5, 0xA9, 0x9D, 0x10, 0xFC},
    //                 },
    //         },
    //     .dev_idx = 0xFF, // 未分配
    //     .connected = false,
    //     .discovery_complete = false,
    //     .name = "V3200",
    //     .retry_count = 0,
    // },
};

#define TEST_DEVICE_COUNT ARRAY_SIZE(test_devices)

#define MAX_CONN_QUEUE 5
static struct {
  bt_addr_le_t addr;
  bool valid;
  int device_index; // 跟踪对应的设备索引
} conn_queue[MAX_CONN_QUEUE];
static int conn_queue_head = 0;
static int conn_queue_tail = 0;
static bool conn_in_progress = false;

K_SEM_DEFINE(conn_sem, 1, 1);

// 添加工作队列处理连接请求
static struct k_work_delayable conn_work;

// 队列管理函数
static bool queue_is_empty(void) {
  return conn_queue_head == conn_queue_tail &&
         !conn_queue[conn_queue_head].valid;
}

static bool queue_is_full(void) {
  return conn_queue_head == conn_queue_tail &&
         conn_queue[conn_queue_head].valid;
}

static void queue_add(const bt_addr_le_t *addr, int device_index) {
  if (queue_is_full()) {
    LOG_ERR("Connection queue is full");
    return;
  }

  bt_addr_le_copy(&conn_queue[conn_queue_tail].addr, addr);
  conn_queue[conn_queue_tail].valid = true;
  conn_queue[conn_queue_tail].device_index = device_index;

  conn_queue_tail = (conn_queue_tail + 1) % MAX_CONN_QUEUE;
}

static bool queue_get(bt_addr_le_t *addr, int *device_index) {
  if (queue_is_empty()) {
    return false;
  }

  bt_addr_le_copy(addr, &conn_queue[conn_queue_head].addr);
  *device_index = conn_queue[conn_queue_head].device_index;
  conn_queue[conn_queue_head].valid = false;

  conn_queue_head = (conn_queue_head + 1) % MAX_CONN_QUEUE;

  return true;
}

// 连接工作处理函数
#if 1
static void process_conn_queue(struct k_work *work) {
  if (conn_in_progress) {
    // 已有连接进行中，延迟处理
    k_work_schedule(&conn_work, K_MSEC(2000));
    return;
  }

  k_sem_take(&conn_sem, K_FOREVER);

  bt_addr_le_t addr;
  int device_idx;
  if (queue_get(&addr, &device_idx)) {
    LOG_INF("Processing connection request for %s",
            test_devices[device_idx].name);

    // 标记连接进行中
    conn_in_progress = true;

    // 使用库的API连接设备
    uint8_t dev_idx;
    int err = ble_central_connect(&addr, &dev_idx);

    if (err && err != BLE_ERR_BUSY) {
      LOG_ERR("Connection creation failed for %s (err %d)",
              test_devices[device_idx].name, err);
      conn_in_progress = false;

      // 如果当前队列不为空，立即处理下一个连接请求
      if (!queue_is_empty()) {
        k_work_schedule(&conn_work, K_MSEC(10));
      } else {
        // 恢复扫描
        bt_le_scan_start(BT_LE_SCAN_PASSIVE, NULL);
      }
    } else if (err == BLE_ERR_BUSY) {
      LOG_INF("Connection already in progress for %s",
              test_devices[device_idx].name);
      conn_in_progress = false;

      // 连接已在进行中，延迟检查
      k_work_schedule(&conn_work, K_SECONDS(1));
    } else {
      // 连接请求成功
      LOG_INF("Connection initiated for %s (device index %d)",
              test_devices[device_idx].name, dev_idx);

      // 保存设备索引
      test_devices[device_idx].dev_idx = dev_idx;

      // 为设备启用自动重连
      ble_central_set_auto_reconnect(dev_idx, true);
      LOG_INF("Auto-reconnect enabled for %s", test_devices[device_idx].name);

      // 不立即处理下一个连接，等待当前连接完成或超时
      // 连接完成或超时会在回调中重置conn_in_progress并触发下一个连接请求
    }
  } else {
    // 队列为空，没有更多的连接请求
    LOG_INF("Connection queue empty");
    bt_le_scan_start(BT_LE_SCAN_PASSIVE, NULL);
  }

  k_sem_give(&conn_sem);
}
#else
static void process_conn_queue(struct k_work *work) {
  if (conn_in_progress) {
    // 已有连接进行中，延迟处理
    k_work_schedule(&conn_work, K_MSEC(2000));
    return;
  }

  k_sem_take(&conn_sem, K_FOREVER);

  bt_addr_le_t addr;
  int device_idx;
  if (queue_get(&addr, &device_idx)) {
    LOG_INF("Processing connection request for device %d", device_idx);

    // 标记连接进行中
    conn_in_progress = true;

    // 停止扫描以提高连接成功率
    bt_le_scan_stop();

    // 预先分配设备索引
    test_devices[device_idx].dev_idx = device_idx;

    // 创建连接
    struct bt_conn *conn = NULL; // 确保初始化为NULL
    int err = bt_conn_le_create(&addr, BT_CONN_LE_CREATE_CONN,
                                BT_LE_CONN_PARAM_DEFAULT, &conn);

    if (err) {
      LOG_ERR("Connection creation failed (err %d)", err);
      conn_in_progress = false;
      test_devices[device_idx].dev_idx = 0xFF; // 重置设备索引

      // 如果当前队列不为空，立即处理下一个连接请求
      if (!queue_is_empty()) {
        k_work_schedule(&conn_work, K_MSEC(10));
      } else {
        // 恢复扫描
        bt_le_scan_start(BT_LE_SCAN_PASSIVE, NULL);
      }
    } else {
      if (conn) {
        LOG_INF("Connection request sent for device %d", device_idx);
        bt_conn_unref(conn); // 释放连接引用
      }

      // 为这个设备启用自动重连
      ble_central_set_auto_reconnect(device_idx, true);
      LOG_INF("Auto-reconnect enabled for device %d", device_idx);

      // 不立即处理下一个连接，等待当前连接完成或超时
      // 连接完成或超时会在回调中重置conn_in_progress并触发下一个连接请求
    }
  } else {
    // 队列为空，恢复扫描
    bt_le_scan_start(BT_LE_SCAN_PASSIVE, NULL);
  }

  k_sem_give(&conn_sem);
}
#endif

// 安排下一个连接请求处理
static void schedule_next_conn(void) {
  k_sem_take(&conn_sem, K_FOREVER);
  conn_in_progress = false;
  k_sem_give(&conn_sem);

  // 安排下一个连接请求处理
  k_work_schedule(&conn_work, K_MSEC(100));
}

// 获取设备索引
static int get_device_info_by_addr(const bt_addr_le_t *addr) {
  for (int i = 0; i < TEST_DEVICE_COUNT; i++) {
    if (bt_addr_le_cmp(addr, &test_devices[i].addr) == 0) {
      return i;
    }
  }
  return -1;
}

// 连接回调
static void on_connected(struct ble_central_dev *dev, void *user_data) {
  char addr_str[BT_ADDR_LE_STR_LEN];
  bt_addr_le_to_str(&dev->addr, addr_str, sizeof(addr_str));

  LOG_INF("Connected to device %d: %s", dev->index, addr_str);

  // 更新设备状态
  int idx = get_device_info_by_addr(&dev->addr);
  if (idx >= 0) {
    test_devices[idx].dev_idx = dev->index;
    test_devices[idx].connected = true;
    test_devices[idx].discovery_complete = false;
    LOG_INF("Identified as %s", test_devices[idx].name);
  }

  // 连接后自动发现服务
  int err = ble_central_discover_services(dev->index);
  if (err) {
    LOG_ERR("Service discovery failed (err %d)", err);
  }

  // 连接完成后，调度下一个连接请求
  schedule_next_conn();
}

// 断开连接回调
static void on_disconnected(struct ble_central_dev *dev, uint8_t reason,
                            void *user_data) {
  char addr_str[BT_ADDR_LE_STR_LEN];
  bt_addr_le_to_str(&dev->addr, addr_str, sizeof(addr_str));

  LOG_INF("Disconnected from device %d: %s (reason %u)", dev->index, addr_str,
          reason);

  // 更新设备状态
  int idx = get_device_info_by_addr(&dev->addr);
  if (idx >= 0) {
    test_devices[idx].connected = false;
    test_devices[idx].discovery_complete = false;
    LOG_INF("%s disconnected", test_devices[idx].name);
  }

  // 如果是连接超时导致的断开，也需要调度下一个连接请求
  if (reason == BT_HCI_ERR_CONN_TIMEOUT) {
    schedule_next_conn();
  }
}

// 数据接收回调
static void on_data_received(struct ble_central_dev *dev,
                             struct ble_gatt_char *characteristic,
                             const uint8_t *data, uint16_t len,
                             void *user_data) {
  // 确定是哪个设备发送的数据
  int idx = get_device_info_by_addr(&dev->addr);
  const char *device_name =
      (idx >= 0) ? test_devices[idx].name : "Unknown device";

  LOG_INF("Received data from %s (device %d), length %u", device_name,
          dev->index, len);

  // 打印数据
  LOG_HEXDUMP_INF(data, len, "Data:");
}

// 连接状态检查
void check_connection_status(void) {
  for (int i = 0; i < TEST_DEVICE_COUNT; i++) {
    if (test_devices[i].dev_idx != 0xFF) {
      bool connecting, connected;
      int err = ble_central_get_connection_state(test_devices[i].dev_idx,
                                                 &connecting, &connected);

      if (err == 0) {
        if (connecting && !connected) {
          LOG_WRN("%s (device %d) still connecting...", test_devices[i].name,
                  test_devices[i].dev_idx);

          // 如果连接长时间没有完成，考虑重置连接状态
          // 添加一个计数器，可以在超过一定次数后重置
          test_devices[i].retry_count++;

          if (test_devices[i].retry_count >
              10) { // 如果连接状态检查10次后仍未连接成功
            LOG_WRN("Resetting connection state for %s", test_devices[i].name);
            ble_central_reset_connection_state(test_devices[i].dev_idx);
            test_devices[i].retry_count = 0;
          }
        } else {
          // 连接成功或完全断开，重置计数器
          test_devices[i].retry_count = 0;
        }

        // 更新测试设备状态
        test_devices[i].connected = connected;

        // 检查发现状态
        struct ble_central_dev *dev =
            ble_central_get_device(test_devices[i].dev_idx);
        if (dev) {
          test_devices[i].discovery_complete = dev->discovery_complete;
        }
      }
    }
  }
}

// 主测试函数
void test_multi_connection(void) {
  // 初始化连接工作队列
  k_work_init_delayable(&conn_work, process_conn_queue);

  // 初始化连接队列
  memset(conn_queue, 0, sizeof(conn_queue));

  // 将设备添加到连接队列
  for (int i = 0; i < TEST_DEVICE_COUNT; i++) {
    queue_add(&test_devices[i].addr, i);
    LOG_INF("Added %s to connection queue", test_devices[i].name);
  }

  // 启动连接处理
  k_work_schedule(&conn_work, K_NO_WAIT);
}

// 发送测试数据
void send_test_data(void) {
  // 不同的测试数据用于不同设备
  static uint8_t test_data1[] = {0x8F, 0xD7, 0x01, 0x00, 0x01, 0x01}; // 设备1
  static uint8_t test_data2[] = {0x8F, 0xD7, 0x02, 0x00, 0x01, 0x01}; // 设备2
  static int counter = 0;

  counter++;

  // 向所有已连接且服务发现完成的设备发送数据
  for (int i = 0; i < TEST_DEVICE_COUNT; i++) {
    if (test_devices[i].connected && test_devices[i].discovery_complete) {
      LOG_INF("Sending data to %s (iteration %d)", test_devices[i].name,
              counter);

      // 根据设备选择不同的测试数据
      uint8_t *data;
      size_t data_len;

      if (i == 0) {
        data = test_data1;
        data_len = sizeof(test_data1);
        // 修改一个字节，使每次发送的数据不同
        // test_data1[5] = (uint8_t)(counter & 0xFF);
      } else {
        data = test_data2;
        data_len = sizeof(test_data2);
        // 修改一个字节，使每次发送的数据不同
        // test_data2[5] = (uint8_t)(counter & 0xFF);
      }

      // 查找服务
      struct ble_gatt_service *svc = ble_central_find_service(
          test_devices[i].dev_idx, BT_UUID_NB_DATA_SERVICE);

      if (svc) {
        printk("send data: ");
        for (int i = 0; i < data_len; i++) {
          printk("%02X", data[i]);
        }
        printk("\n");
        // 发送数据
        int err = ble_central_write_characteristic(
            test_devices[i].dev_idx, BT_UUID_NB_DATA_SERVICE,
            BT_UUID_NB_DATA_SERVICE_WRITE, data, data_len, false);

        if (err) {
          LOG_ERR("Write failed for %s (err %d)", test_devices[i].name, err);
        } else {
          LOG_INF("Data sent to %s", test_devices[i].name);
        }
      } else {
        LOG_ERR("Service not found for %s", test_devices[i].name);
      }
    } else if (test_devices[i].connected) {
      LOG_WRN("%s connected but discovery not complete", test_devices[i].name);
    }
  }
}

// 测试断开和重连逻辑
void test_disconnect_reconnect(void) {
  static int test_cycle = 0;

  test_cycle++;

  // 每10个循环执行一次断开测试
  if (test_cycle % 10 == 0) {
    for (int i = 0; i < TEST_DEVICE_COUNT; i++) {
      // 轮流断开设备
      if (test_devices[i].connected &&
          (test_cycle / 10) % TEST_DEVICE_COUNT == i) {
        LOG_INF("Test: Disconnecting %s temporarily...", test_devices[i].name);
        ble_central_disconnect(test_devices[i].dev_idx);
        break; // 每次只断开一个设备
      }
    }
  }
}

int main(void) {
  int err;

  // 初始化BLE堆栈
  err = bt_enable(NULL);
  if (err) {
    LOG_ERR("Bluetooth init failed (err %d)", err);
    return err;
  }

  // 配置多连接中心设备库
  struct ble_central_config config = {.target_service_uuids = target_services,
                                      .service_uuid_count =
                                          ARRAY_SIZE(target_services),
                                      .connect_cb = on_connected,
                                      .disconnect_cb = on_disconnected,
                                      .data_cb = on_data_received,
                                      .user_data = NULL};

  // 初始化库
  err = ble_central_init(&config);
  if (err) {
    LOG_ERR("BLE Central init failed (err %d)", err);
    return err;
  }

  // 开始扫描
  err = ble_central_start_scan();
  if (err) {
    LOG_ERR("Start scan failed (err %d)", err);
    return err;
  }

  LOG_INF("BLE Central multi-device test started");

  // 初始化连接
  test_multi_connection();

  // 主循环
  while (1) {
    k_sleep(K_SECONDS(2));

    // 获取已连接设备数量
    uint8_t count = ble_central_get_connected_count();
    LOG_INF("Connected devices: %u / %d", count, TEST_DEVICE_COUNT);

    // 检查连接状态
    check_connection_status();

    // 发送测试数据
    send_test_data();

    // 测试断开/重连功能
    test_disconnect_reconnect();
  }

  return 0;
}
